//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================

#include <core.h>
#include <_hal.h>
#include <_context.h>
#include <init.h>
#include <entry.h>

//
// ThreadHContext
//
// TODO:

//
// Global function definitions
//
typedef struct ThreadSwitchContext {
    uint32_t    cpsr;
    uint32_t    r4, r5, r6, r7, r8, r9, r10, r11, pc;
} ThreadSwitchContext;

#if defined(_GNUC)

EXTERN_C void __ThreadContextSwitch(
    Thread *pPrevThread, Thread *pNextThread)
{
DECL_ASMENTRY(ThreadContextSwitch)
    ASM("stmfd  sp!, {r4 - r11, lr};"
        "mrs    ip, cpsr;"
        "str    ip, [sp, #-4]!;"
        "str    sp, [r0, %0];"
        "ldr    sp, [r1, %0];"
        "ldr    ip, [sp], #4;"
        "msr    spsr, ip;"
        "ldmfd  sp!, {r4 - r11, pc}^;"
        : :"i"(offsetof(Thread, m_vaKernelStackPtr)));
}

EXTERN_C void __ThreadContextSwitchNoReturn(Thread *pNextThread)
{
DECL_ASMENTRY(ThreadContextSwitchNoReturn)
    ASM("ldr    sp, [r0, %0];"
        "ldr    ip, [sp], #4;"
        "msr    spsr, ip;"
        "ldmfd  sp!, {r4 - r11, pc}^;"
        : :"i"(offsetof(Thread, m_vaKernelStackPtr)));
}

#endif // defined(_GNUC)

#define INIT_SVC_CPSR   PSR_MODE_SVC
#define INIT_USR_CPSR   PSR_MODE_USR

EXTERN virtaddr_t InitThread0Stack(Thread *pThread)
{
    return (virtaddr_t *)pThread->m_vaKernelStack;
}

//
// The layout of new internal kernel thread stack is:
//
// thread.m_vaKernelStack ->      |                           |
//                                +---------------------------+
//                                | (pc: SystemThreadStartup) |
//                                | (r6: threadentry)         |
//                                | (r5: pvParameter)         |
//                                | (r4: threadroutine)       |
//                                |                           |
//                                |                           |
//                                |    ThreadSwitchContext    |
// thread.m_vaKernelStackPtr ->   |                           |
//                                +---------------------------+

EXTERN virtaddr_t InitSystemThreadStack(
    Thread *pThread, threadentry_t threadentry,
    threadroutine_t threadroutine, void *pvParameter)
{
    ThreadSwitchContext *pTSContext =
        (ThreadSwitchContext *)pThread->m_vaKernelStack - 1;

    pTSContext->pc      = (uint32_t)&SystemThreadStartup;
    pTSContext->r11     = 0;
    pTSContext->r6      = (uint32_t)threadentry;
    pTSContext->r5      = (uint32_t)pvParameter;
    pTSContext->r4      = (uint32_t)threadroutine;
    pTSContext->cpsr    = INIT_SVC_CPSR;

    return (virtaddr_t)pTSContext;
}

//
// The layout of new kernel thread stack is:
//
// thread.m_vaKernelStack ->      |                           |
//                                +---------------------------+
//                                |                           |
//                                |     InterruptContext      |
//                                |                           |
//                                +---------------------------+
//                                |    (pc: ThreadStartup)    |
//                                |    (r5: pvParameter)      |
//                                |    (r4: threadroutine)    |
//                                |                           |
//                                |                           |
//                                |    ThreadSwitchContext    |
// thread.m_vaKernelStackPtr ->   |                           |
//                                +---------------------------+
//

EXTERN virtaddr_t InitThreadStack(Thread *pThread,
    threadroutine_t threadroutine, void *pvParameter)
{
    InterruptContext *pIContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

    ThreadSwitchContext *pTSContext =
        (ThreadSwitchContext *)pIContext - 1;

    pTSContext->pc      = (uint32_t)&ThreadStartup;
    pTSContext->r11     = 0;
    pTSContext->r5      = (uint32_t)pvParameter;
    pTSContext->r4      = (uint32_t)threadroutine;
    pTSContext->cpsr    = INIT_SVC_CPSR;

    return (virtaddr_t)pTSContext;
}

EXTERN void SetThreadUserEntry(Thread *pThread,
    virtaddr_t vaUserStack, virtaddr_t vaUserEntry)
{
    InterruptContext *pIContext =
        (InterruptContext *)pThread->m_vaKernelStack - 1;

    pIContext->pc       = (uint32_t)vaUserEntry;
    pIContext->r11      = 0;
    pIContext->cpsr     = INIT_USR_CPSR;
    pIContext->sp       = (uint32_t)vaUserStack;
    // The second argument is the alternative sp
    pIContext->r1       = pIContext->sp; // r1 = alternative sp
}

//
// ProcessHContext
//
ECode ProcessHContext::SetBreakPoint(uint_t uNo,
    BreakPointType type, void *pvAddress, uint_t uLength)
{
#ifdef KCONFIG_GLOBAL_BREAKPOINT
    return E_NOT_IMPLEMENTED;
#else
    return E_NOT_IMPLEMENTED;
#endif // KCONFIG_GLOBAL_BREAKPOINT
}

#ifdef KCONFIG_GLOBAL_BREAKPOINT
EXTERN ECode SetGlobalBreakPoint(uint_t uNo,
    BreakPointType type, void *pvAddress, uint_t uLength)
{
    return E_NOT_IMPLEMENTED;
}
#endif // KCONFIG_GLOBAL_BREAKPOINT

#ifdef _wmmx
EXTERN_C void SaveWMMXRegisters(ulong_t *pMmxData, uint_t *pFlags);
EXTERN_C void RestoreWMMXRegisters(ulong_t *pMmxData);

INLINE void ThreadHContext::SaveWMMX()
{
    SaveWMMXRegisters(pMmxData, &m_uMmxFlags);
}

INLINE void ThreadHContext::RestoreWMMX()
{
    // ThreadHContextFlag_WMMXUsed is set in wmmx.S
    if (m_uMmxFlags) {
        RestoreWMMXRegisters(pMmxData);
    }
}
#endif // _wmmx

void ThreadHContext::Save()
{
    m_uFaultStatus = GetFaultStatus();
    m_vaFaultAddress = GetFaultAddress();

#ifdef _wmmx
    SaveWMMX();
#endif
}

void ThreadHContext::Restore()
{
    SetFaultStatus(m_uFaultStatus);
    SetFaultAddress(m_vaFaultAddress);

#ifdef _wmmx
    RestoreWMMX();
#endif
}
